time = 0:Ts/4:Duration/4;
figure(2);
subplot(4,1,1);
plot(time, xHistory(:,1),'LineWidth',2);
hold on;
grid on;
plot(time(1:size(x_refHistory,[1])), x_refHistory(:,1),'LineStyle','--');
xlabel('x(m)');
legend('实际轨迹','期望轨迹');
subplot(4,1,2);
plot(time, xHistory(:,2),'LineWidth',2);
hold on;
grid on;
plot(time(1:size(x_refHistory,[1])), x_refHistory(:,2),'LineStyle','--');
xlabel('y(m)');
legend('实际轨迹','期望轨迹');
subplot(4,1,3);
plot(time, xHistory(:,3),'LineWidth',2);
hold on;
grid on;
plot(time(1:size(x_refHistory,[1])), x_refHistory(:,3),'LineStyle','--');
xlabel('z(m)');
legend('实际轨迹','期望轨迹');
subplot(4,1,4);
plot(time, xHistory(:,6),'LineWidth',2);
hold on;
grid on;
plot(time(1:size(x_refHistory,[1])), x_refHistory(:,6),'LineStyle','--');
xlabel('yaw(rad)');
legend('实际轨迹','期望轨迹');
figure(3);
plot3(xHistory(:,1),xHistory(:,2),xHistory(:,3),'LineStyle','-','LineWidth',2);
hold on;
grid on;
plot3(x_refHistory(:,1),x_refHistory(:,2),x_refHistory(:,3),'LineStyle','--');
legend('实际轨迹','期望轨迹');

figure(4);
subplot(4,1,1);
plot(time(1:size(x_refHistory,[1])), x_refHistory(:,1)-xHistory(1:size(x_refHistory,[1]),1),'LineWidth',2);
grid on;
xlabel('Error in X(m)');
subplot(4,1,2);
plot(time(1:size(x_refHistory,[1])), x_refHistory(:,2)-xHistory(1:size(x_refHistory,[1]),2),'LineWidth',2);
grid on;
xlabel('Error in Y(m)');
subplot(4,1,3);
plot(time(1:size(x_refHistory,[1])), x_refHistory(:,3)-xHistory(1:size(x_refHistory,[1]),3),'LineWidth',2);
grid on;
xlabel('Error in Z(m)');
subplot(4,1,4);
plot(time(1:size(x_refHistory,[1])), x_refHistory(:,6)-xHistory(1:size(x_refHistory,[1]),6),'LineWidth',2);
grid on;
xlabel('Error in Yaw(rad)');
figure(5);
subplot(4,1,1);
plot(time, uHistory(:,1),'LineWidth',2);
grid on;
xlabel('U1(N)');
subplot(4,1,2);
plot(time, uHistory(:,2),'LineWidth',2);
grid on;
xlabel('U2(N·m)');
subplot(4,1,3);
plot(time, uHistory(:,3),'LineWidth',2);
grid on;
xlabel('U3(N·m)');
subplot(4,1,4);
plot(time, uHistory(:,4),'LineWidth',2);
grid on;
xlabel('U4(N·m)');